from machine import Pin, I2C, Timer
import time
import gc
from hid_services import Mouse  #https://github.com/Heerkog/MicroPythonBLEHID

# ESP32-WROOM-32 devkit 1 接线
# Pin 34：用于左按钮输入，配置为带上拉电阻的输入引脚。
# Pin 13：用于右按钮输入，同样配置为带上拉电阻的输入引脚。
# Pin 32：用于I2C数据线（SDA）。
# Pin 33：用于I2C时钟线（SCL）。

# 调试变量
DEBUG_VARIABLES = {
    'MOVEMENT_THRESHOLD': 2,  # 注册为鼠标移动所需的最小移动量死区
    'MAX_MOVEMENT': 15,       # 单次更新允许的最大移动量
    'SENSITIVITY': 1.4,       # 鼠标移动灵敏度乘数
    'TEMP_COMPENSATION_GYRO_X': 0.01,  # X轴陀螺仪温度补偿因子
    'TEMP_COMPENSATION_GYRO_Y': 0.01,  # Y轴陀螺仪温度补偿因子
    'TEMP_COMPENSATION_GYRO_Z': 0.01,  # Z轴陀螺仪温度补偿因子
    'CONNECTED_SLEEP_MS': 10,  # 连接时更新间隔的睡眠时间（毫秒）
    'IDLE_SLEEP_MS': 500,     # 空闲时更新间隔的睡眠时间（毫秒）
    'UPDATE_INTERVAL_MS': 10,  # 更新间隔（毫秒）
}

class MPU6050:
    def __init__(self, i2c, addr=0x68):
        self.i2c = i2c
        self.addr = addr
        self._init_sensor()

    def _init_sensor(self):
        # 初始化MPU6050传感器
        try:
            self.i2c.writeto_mem(self.addr, 0x6B, bytearray([0]))  # 唤醒传感器
            time.sleep(0.1)
            self.i2c.writeto_mem(self.addr, 0x1C, bytearray([0]))  # 设置加速度计量程
            time.sleep(0.1)
            self.i2c.writeto_mem(self.addr, 0x1B, bytearray([0]))  # 设置陀螺仪量程
            time.sleep(0.1)
            self.i2c.writeto_mem(self.addr, 0x19, bytearray([7]))  # 设置采样率
        except Exception as e:
            print("MPU6050初始化失败:", e)

    def _read_raw_data(self, reg):
        # 读取原始数据
        try:
            high = self.i2c.readfrom_mem(self.addr, reg, 1)[0]
            low = self.i2c.readfrom_mem(self.addr, reg + 1, 1)[0]
            value = (high << 8) | low
            if value > 32768:
                value -= 65536
            return value
        except Exception as e:
            print("读取传感器数据失败:", e)
            return 0

    def get_gyro_data(self):
        # 获取陀螺仪数据
        gx = self._read_raw_data(0x43) / 131.0
        gy = self._read_raw_data(0x45) / 131.0
        gz = self._read_raw_data(0x47) / 131.0
        return {'x': gx, 'y': gy, 'z': gz}

    def get_temp(self):
        # 获取温度数据
        temp_raw = self._read_raw_data(0x41)
        temp_c = (temp_raw / 340.0) + 36.53
        return temp_c

class Device:
    def __init__(self):
        self.debug_vars = DEBUG_VARIABLES
        self.last_report_time = 0


        self.gc_counter = 0
        self.GC_INTERVAL = 50  # 每100次循环进行一次GC

        # 初始化按钮引脚
        self.pin_left_button = Pin(34, Pin.IN, Pin.PULL_UP)
        self.pin_right_button = Pin(13, Pin.IN, Pin.PULL_UP)

        # 初始化按钮状态
        self.button_states = {'left': False, 'right': False}
        self.button_state_changed = False
        self.left_last_change_time = 0
        self.right_last_change_time = 0
        self.debounce_delay = 25  # 去抖动延迟（毫秒）

        # 初始化I2C和MPU6050
        i2c = I2C(0, sda=Pin(32), scl=Pin(33), freq=40000)
        self.mpu = MPU6050(i2c)

        # 初始化蓝牙HID鼠标
        self.mouse = Mouse("Mouse")
        self.mouse.set_state_change_callback(self.mouse_state_callback)

        # 设置按钮中断
        self.pin_left_button.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=self.left_button_handler)
        self.pin_right_button.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=self.right_button_handler)

        # 初始化累积移动量
        self.accumulated_dx = 0
        self.accumulated_dy = 0


        # 初始化卡尔曼滤波器参数
        self.kalman_x = {'x': 0, 'P': 1, 'Q': 0.1, 'R': 0.9}
        self.kalman_y = {'x': 0, 'P': 1, 'Q': 0.1, 'R': 0.7}


        # 初始化累积移动量
        self.accumulated_dx = 0
        self.accumulated_dy = 0

    def mouse_state_callback(self):
        state = self.mouse.get_state()
        if state == Mouse.DEVICE_IDLE:
            print("鼠标空闲")
        elif state == Mouse.DEVICE_ADVERTISING:
            print("鼠标正在广播")
        elif state == Mouse.DEVICE_CONNECTED:
            print("鼠标已连接")
        else:
            print("未知的鼠标状态")

    def kalman_filter(self, kalman, measurement):
        # 预测
        kalman['P'] += kalman['Q']

        # 更新
        K = kalman['P'] / (kalman['P'] + kalman['R'])
        kalman['x'] += K * (measurement - kalman['x'])
        kalman['P'] *= (1 - K)

        return kalman['x']

    def get_mpu_movement(self):
        gyro = self.mpu.get_gyro_data()
        temp = self.mpu.get_temp()
        
        # 应用温度补偿
        gyro['x'] -= self.debug_vars['TEMP_COMPENSATION_GYRO_X'] * (temp - 36.53)
        gyro['y'] -= self.debug_vars['TEMP_COMPENSATION_GYRO_Y'] * (temp - 36.53)
        gyro['z'] -= self.debug_vars['TEMP_COMPENSATION_GYRO_Z'] * (temp - 36.53)
        
        # 计算移动量
        dx = int(gyro['y'] * self.debug_vars['SENSITIVITY'])
        dy = int(-gyro['x'] * self.debug_vars['SENSITIVITY'])
        
        # 应用死区
        if abs(dx) <= self.debug_vars['MOVEMENT_THRESHOLD']:
            dx = 0
        if abs(dy) <= self.debug_vars['MOVEMENT_THRESHOLD']:
            dy = 0
        
        # 应用移动阈值
        if abs(dx) < self.debug_vars['MOVEMENT_THRESHOLD']:
            dx = 0
        if abs(dy) < self.debug_vars['MOVEMENT_THRESHOLD']:
            dy = 0
        
        # 限制最大移动量
        dx = max(min(dx, self.debug_vars['MAX_MOVEMENT']), -self.debug_vars['MAX_MOVEMENT'])
        dy = max(min(dy, self.debug_vars['MAX_MOVEMENT']), -self.debug_vars['MAX_MOVEMENT'])
        
        # 应用卡尔曼滤波
        filtered_dx = self.kalman_filter(self.kalman_x, dx)
        filtered_dy = self.kalman_filter(self.kalman_y, dy)
        
        return int(filtered_dx), int(filtered_dy)

    def left_button_handler(self, pin):
        current_time = time.ticks_ms()
        if time.ticks_diff(current_time, self.left_last_change_time) > self.debounce_delay:
            new_state = pin.value() == 1  # 高电平为按下
            if new_state != self.button_states['left']:
                self.button_states['left'] = new_state
                self.left_last_change_time = current_time
                self.button_state_changed = True
                self.update_button_state()

    def right_button_handler(self, pin):
        current_time = time.ticks_ms()
        if time.ticks_diff(current_time, self.right_last_change_time) > self.debounce_delay:
            new_state = pin.value() == 1  # 高电平为按下
            if new_state != self.button_states['right']:
                self.button_states['right'] = new_state
                self.right_last_change_time = current_time
                self.button_state_changed = True
                self.update_button_state()

    def update_button_state(self):
        if self.mouse.is_connected():
            try:
                self.mouse.set_buttons(self.button_states['left'], self.button_states['right'])
                self.mouse.notify_hid_report()
                #print(f"Button state updated: Left={self.button_states['left']}, Right={self.button_states['right']}")
            except OSError as e:
                print("按钮状态更新错误:", e)

    def start(self):
        # 启动蓝牙广播
        self.mouse.start()
        self.mouse.start_advertising()

        while True:
            current_time = time.ticks_ms()
            if self.mouse.is_connected():
                dx, dy = self.get_mpu_movement()

                # 累积移动量
                self.accumulated_dx += dx
                self.accumulated_dy += dy

                # 判断是否需要发送HID报告
                time_since_last_report = time.ticks_diff(current_time, self.last_report_time)
                should_send_report = (
                    self.accumulated_dx != 0 or
                    self.accumulated_dy != 0 or
                    self.button_state_changed or
                    time_since_last_report >= self.debug_vars['UPDATE_INTERVAL_MS']
                )

                if should_send_report:
                    try:
                        # 发送包含移动和按钮状态的HID报告
                        self.mouse.set_axes(self.accumulated_dx, self.accumulated_dy)
                        self.mouse.set_buttons(self.button_states['left'], self.button_states['right'])
                        self.mouse.notify_hid_report()
                        self.last_report_time = current_time
                        self.accumulated_dx = 0
                        self.accumulated_dy = 0
                        self.button_state_changed = False
                        #print(f"HID report sent: dx={self.accumulated_dx}, dy={self.accumulated_dy}, Left={self.button_states['left']}, Right={self.button_states['right']}")
                    except OSError as e:
                        print("HID报告通知错误:", e)

            else:
                if self.mouse.get_state() != Mouse.DEVICE_ADVERTISING:
                    print("开始广播")
                    self.mouse.start_advertising()
                time.sleep_ms(self.debug_vars['IDLE_SLEEP_MS'])
            
            # 添加固定的睡眠时间来控制循环频率
            time.sleep_ms(self.debug_vars['CONNECTED_SLEEP_MS'])

            # 定期执行垃圾回收
            self.gc_counter += 1
            if self.gc_counter >= self.GC_INTERVAL:
                gc.collect()
                self.gc_counter = 0
if __name__ == "__main__":
    device = Device()
    device.start()